package ch.ethz.fcl.tools;

import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;

import ch.ethz.fcl.Constant;

public class Tools {
	// return the centroid of a curve
	public static Point centroid(MatOfPoint curve) {
		Point[] points = curve.toArray();
		double x = 0, y = 0;
		for (int i = 0; i < points.length; i++) {
			x += points[i].x / points.length;
			y += points[i].y / points.length;
		}

		return new Point(x, y);
	}

	public static float dist(double x1, double y1, double x2, double y2) {
		return (float) Math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
	}

	public static float dist(Point p1, Point p2) {
		return (float) Math.sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y)
				* (p1.y - p2.y));
	}

	public static Point center(Point... pts) {
		double x = 0, y = 0;
		for (int i = 0; i < pts.length; i++) {
			x += pts[i].x;
			y += pts[i].y;
		}

		return new Point(x / pts.length, y / pts.length);
	}

	public static double radToDegree(double rad) {
		return rad * 180 / Math.PI;
	}

	public static double degreeToRad(double degree) {
		return degree * Math.PI / 180;
	}

	public static double mergeDegree(double degree1, double weight1,
			double degree2, double weight2) {
		if (degree1 == 0)
			return degree2;

		double sub = Math.abs(degree1 - degree2);
		double angleDegree = 0;
		if (sub > 90 - Constant.ANGLE_THRE && sub < 90 + Constant.ANGLE_THRE) {
			angleDegree = (90 + degree2) % 180;
		}

		else if (sub < Constant.ANGLE_THRE || sub > (180 - Constant.ANGLE_THRE)) {
			angleDegree = degree2;
		}

		if (degree1 - angleDegree > (180 - Constant.ANGLE_THRE))
			angleDegree = angleDegree + 180;
		else if (angleDegree - degree1 > (180 - Constant.ANGLE_THRE))
			angleDegree = angleDegree - 180;

		// System.out.println("angle is " + degree2 + " new angleDegree is "
		// + angleDegree);
		return (weight1 * degree1 + angleDegree * weight2)
				/ (weight1 + weight2);

	}

	/**
	 * 
	 * @return the angle difference between the principle axis and the input
	 *         angle
	 */
	public static double angleDiff(double prinAxisDegree, double angleDegree) {
		double angleDiff = Double.MAX_VALUE;
		double sub = Math.abs(prinAxisDegree - angleDegree);
		// if parallel, between (0, thre) or (180 - thre, 180)
		if (sub < Constant.ANGLE_THRE || sub > (180 - Constant.ANGLE_THRE)) {
			angleDiff = sub;
		}
		// if perpendicular and between (90 - thre, 90 + thre)
		else if (sub > 90 - Constant.ANGLE_THRE
				&& sub < 90 + Constant.ANGLE_THRE) {
			angleDiff = Math.abs(90 - sub);
		}

		return angleDiff;
	}

	public static float inBound(float in, float inMin, float inMax) {
		if (in > inMax)
			return inMax;
		else if (in < inMin)
			return inMin;

		return in;
	}
}
